/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : mahonyAhrs.cpp
  * Description        : 
  ******************************************************************************
  * Function           :
  *
  ******************************************************************************
  * Author             : Zhiping Fu
  * Creation Date      : 2020年11月20日
  ******************************************************************************
  */
/* USER CODE END Header */
#include "mahonyAhrs.hpp"


void MAHONYAHRS::mahonyAhrs_init(void)
{
	OneG = 9.788f;
	Update = false;
	AngOff[0] = 0.0f;   //往左飘增大该系数
	AngOff[1] = 0.0f;   //往左飘增大该系数
	AngOff[2] = 0.0f;   //往左飘增大该系数
	Dt = 0.001f;//

	Kp = 0.3f;
	Ki = 0.0001f;

	osDelay(180);						//等待传感器数据
	/*求状态变量初始值*/
	//获取IMU+MAG数据
	xQueuePeek(queueGyrDat, &gyro, 0);					//从队列中获取陀螺仪数据
	xQueuePeek(queueAccDat, &acc, 0);					//从队列中获取加速度计数据
	xQueuePeek(queueMagDat, &mag, 0);					//从队列中获取磁力计数据
	/***************gyro角速度/acc加速度计/mag磁场强度获取*****************/
	MatGyr << gyro.gyro[0],gyro.gyro[1],gyro.gyro[2];	//Unit:rad/s
	MatAcc << -acc.acc[0],-acc.acc[1],-acc.acc[2];			//Unit:m/s2
	MatMag << mag.mag[0],mag.mag[1],mag.mag[2];			//Unit:uT
	//Acc、Mag数据归一化
	MatAcc.normalize();
	MatMag.normalize();

	Euler[0] = atan2f(MatAcc(1),MatAcc(2));
	Euler[1] = -asinf(MatAcc(0));

	float mag_calib[2];
	float sinR,cosR,sinP,cosP;
	sinR = sinf(Euler[0]);
	cosR = cosf(Euler[0]);
	sinP = sinf(Euler[1]);
	cosP = cosf(Euler[1]);

	mag_calib[0]=-MatMag(1)*cosR + MatMag(2)*sinR;//分子
	mag_calib[1]=MatMag(0)*cosP + MatMag(1)*sinP*sinR + MatMag(2)*sinP*cosR;//分母

	if (mag_calib[1] != 0)
	{
		Euler[2] = atan2f(mag_calib[0], mag_calib[1]);
	}
	else
	{
		if (mag_calib[0] < 0)
			Euler[2] = -M_PI/2;
		else
			Euler[2] = M_PI/2;
	}
	//欧拉角转四元数,转旋转矩阵
	q = AngleAxisf(Euler(2),Vector3f::UnitZ())*AngleAxisf(Euler(1),Vector3f::UnitY())*AngleAxisf(Euler(0),Vector3f::UnitX());

	q.normalize();


//	Cb2n << 1.0f-2.0f*(q.y()*q.y() + q.z()*q.z()),2.0f*(q.x()*q.y() - q.w()*q.z())     ,2.0f*(q.x()*q.z() + q.w()*q.y()),
//			2.0f*(q.x()*q.y() + q.w()*q.z())     ,1.0f-2.0f*(q.x()*q.x() + q.z()*q.z()),2.0f*(q.y()*q.z() - q.w()*q.x()),
//			2.0f*(q.x()*q.z() - q.w()*q.y())     ,2.0f*(q.y()*q.z() + q.w()*q.x())     ,1.0f-2.0f*(q.x()*q.x() + q.y()*q.y());

	Cb2n = q.toRotationMatrix();

}

void MAHONYAHRS::mahonyAhrs_run()
{
	getTimer_us(&startTimer);
	/* 获取加速度计 陀螺仪和磁力计数据 */
	xQueuePeek(queueGyrDat, &gyro, 0);					//从队列中获取陀螺仪数据
	xQueuePeek(queueAccDat, &acc, 0);					//从队列中获取加速度计数据
	xQueuePeek(queueMagDat, &mag, 0);					//从队列中获取磁力计数据

	MatGyr << gyro.gyro[0],gyro.gyro[1],gyro.gyro[2];	//Unit:rad/s
	MatAcc << -acc.acc[0],-acc.acc[1],-acc.acc[2];			//Unit:m/s2
	MatMag << mag.mag[0],mag.mag[1],mag.mag[2];			//Unit:uT
	/* 获取加速度计 陀螺仪和磁力计数据  */

	//Acc、Mag数据归一化
	MatAcc.normalize();
//	float ax = MatAcc(0), ay = MatAcc(1), az = MatAcc(2);
	// 重力实测的重力向量
	//载体坐标系下
	MatMag.normalize();

	// 上一个四元数解算出来的重力向量，伪常量。由Cb2n*gb=gn=[0;0;g]推出
	//gb=Cn2b*gn=Cn2b最后一列乘以g  即Cb2n最后一行乘以g
	//载体坐标系下
	gb << Cb2n(2,0),Cb2n(2,1),Cb2n(2,2);

	// 计算参考地磁方向，伪常量（ 上一个四元数解算出来）
	//b->n(载体->导航)  hn=Cb2n*mb
	hn = Cb2n*MatMag;
	bn << sqrtf((hn(0)*hn(0)) + (hn(1)*hn(1))),0.0f,hn(2);
	// 计算常量在机体坐标下的表示。m到h到b到r，参考https://blog.csdn.net/hongbin_xu/article/details/59110226
	//n->b(导航->载体) rb=Cn2b*bn=(Cb2n)^T*bn
	rb = Cb2n.transpose()*bn;

	float compass = 1.0f;
	float acc = 1.0f;

//		if(ak8975->Update == false)compass = 0.0f;//
	//叉乘得到误差 对陀螺仪的补偿校正误差向量 磁传感器表征的误差向量
	e(0) = (MatAcc(1)*gb(2) - MatAcc(2)*gb(1))*acc;
	e(1) = (MatAcc(2)*gb(0) - MatAcc(0)*gb(2))*acc;
	e(2) = (MatMag(0)*rb(1) - MatMag(1)*rb(0))*compass;
	eInt = eInt + e;
	w_rad = MatGyr + e*Kp + eInt*Ki;

	// 融合，四元数乘法。[注][2016-1-18]实质上也是四元素微分方程的毕卡求解法之一阶近似算法
	//(q(k+1)-q(k))/Dt=1/2*[0 -wx -wy -wz;wx 0 wz -wy;wz wy -wx 0]*q(k)
	//q(k+1)= q(k) + [0 -wx -wy -wz;wx 0 wz -wy;wz wy -wx 0]*q(k)*halfDt ,其中w即补偿过的陀螺仪数据g , halfDt=Dt/2

	q = q * AngleAxisf(w_rad(2)*Dt,Vector3f::UnitZ())*AngleAxisf(w_rad(1)*Dt,Vector3f::UnitY())*AngleAxisf(w_rad(0)*Dt,Vector3f::UnitX());

	// 规范化四元数，防止发散。
	q.normalize();
//	Cb2n << 1.0f-2.0f*(q.y()*q.y() + q.z()*q.z()),2.0f*(q.x()*q.y() - q.w()*q.z())     ,2.0f*(q.x()*q.z() + q.w()*q.y()),
//			2.0f*(q.x()*q.y() + q.w()*q.z())     ,1.0f-2.0f*(q.x()*q.x() + q.z()*q.z()),2.0f*(q.y()*q.z() - q.w()*q.x()),
//			2.0f*(q.x()*q.z() - q.w()*q.y())     ,2.0f*(q.y()*q.z() + q.w()*q.x())     ,1.0f-2.0f*(q.x()*q.x() + q.y()*q.y());
	Cb2n = q.toRotationMatrix();

	//四元数转欧拉角
	mahonyAhrs_Ang.Ang[0] =  atan2f(2.0f * q.y() * q.z() + 2.0f * q.w() * q.x(), -2.0f * q.x() * q.x() - 2.0f * q.y() * q.y() + 1.0f) -  AngOff[0]*D2R;
	mahonyAhrs_Ang.Ang[1] =  asinf(fConstrain(-2.0f * q.x() * q.z() + 2.0f * q.w() * q.y(),-1.0f,1.0f)) - AngOff[1]*D2R;
	mahonyAhrs_Ang.Ang[2] =  atan2f(2.0f * q.x() * q.y() + 2.0f * q.w() * q.z(), -2.0f * q.y() * q.y() - 2.0f * q.z() * q.z() + 1.0f) -  AngOff[2]*D2R;

	MatCb2n.Cb2n[0][0]  = Cb2n(0,0);
	MatCb2n.Cb2n[0][1]  = Cb2n(0,1);
	MatCb2n.Cb2n[0][2]  = Cb2n(0,2);
	MatCb2n.Cb2n[1][0]  = Cb2n(1,0);
	MatCb2n.Cb2n[1][1]  = Cb2n(1,1);
	MatCb2n.Cb2n[1][2]  = Cb2n(1,2);
	MatCb2n.Cb2n[2][0]  = Cb2n(2,0);
	MatCb2n.Cb2n[2][1]  = Cb2n(2,1);
	MatCb2n.Cb2n[2][2]  = Cb2n(2,2);

	Update = true;
	xQueueOverwrite(queueMahonyAhrs,&mahonyAhrs_Ang);
	xQueueOverwrite(queueCb2n,&MatCb2n);

	getTimer_us(&stopTimer);
	executionTime_us = stopTimer - startTimer;


}

MAHONYAHRS mahonyAhrs((char *)"mahonyAhrs");

extern "C" void mahonyAhrs_main(void *argument)
{
	mahonyAhrs.mahonyAhrs_init();
	osDelay(1);
	for(;;)
	{
		osSemaphoreAcquire(semMahonyAhrs,0xffffffff);
		mahonyAhrs.mahonyAhrs_run();
	}
}


/************************ (C) COPYRIGHT Longmen Drone Team *****END OF FILE****/
